/*
 * downwash.cpp
 *
 *  Created on: Apr 29, 2011
 *      Author: tflanzer
 */

#include <math.h>
#include <iostream>
#include "Aircraft.h"
#include "Matrix.h"
#include "Vector1D.h"
using namespace std;

void indvel(Vector1D xa, Vector1D ya, Vector1D za, Vector1D xb, Vector1D yb,
		Vector1D zb, Vector1D xc, Vector1D yc, Vector1D zc, Matrix* u, Matrix* v,
		Matrix* w, int m, int n);

void downwash(acStruct &ac, colTot &ct) {
	 Matrix qcm(MAXELEM,3),	tem(MAXELEM,3),	cpm(MAXELEM,3),	udw(MAXELEM,MAXELEM),
			vdw(MAXELEM,MAXELEM), wdw(MAXELEM,MAXELEM),	du(MAXELEM,MAXELEM),
			dv(MAXELEM,MAXELEM), dw(MAXELEM,MAXELEM), AIC(MAXELEM,MAXELEM);
	Vector1D dihm(MAXELEM), qcusm(MAXELEM), dwvec(3);
	int mqc[20], nqc[20], mcp[20], ncp[20], mtqc[20], mtcp[20], idx,
		idq, idqp1, idc, idcp1, idcj, idcjp1;
	double dx;

	udw.initConst(0.0); du.initConst(0.0);
	vdw.initConst(0.0); dv.initConst(0.0);
	wdw.initConst(0.0); dw.initConst(0.0);

	mtqc[0] = 0; mtcp[0] = 0;
	for (int i = 0; i < NELEMENTS; i++) {
        mqc[i] = ct.celem[i].N+1;
        nqc[i] = 3;
        mcp[i] = ct.celem[i].N;
        ncp[i] = 3;
        idx = 0;
        for (int j = mtqc[i]; j < mtqc[i]+mqc[i]; j++) {
        	qcm(j,0) = ct.celem[i].qc[idx][0];
        	qcm(j,1) = ct.celem[i].qc[idx][1];
        	qcm(j,2) = ct.celem[i].qc[idx][2];
        	tem(j,0) = ct.celem[i].te[idx][0];
        	tem(j,1) = ct.celem[i].te[idx][1];
        	tem(j,2) = ct.celem[i].te[idx][2]; idx++;
        }
        idx = 0;
        for (int j = mtcp[i]; j < mtcp[i]+mcp[i]; j++) {
        	cpm(j,0) = ct.celem[i].cp[idx][0];
        	cpm(j,1) = ct.celem[i].cp[idx][1];
        	cpm(j,2) = ct.celem[i].cp[idx][2];
        	dihm(j) = ct.celem[i].di[idx];
        	qcusm(j) = ct.celem[i].qcusx[idx]; idx++;
        }

        mtqc[i+1] = mtqc[i] + mqc[i];
        mtcp[i+1] = mtcp[i] + mcp[i];
	}

	// Compute velocities at all control points induced by each panel
	for (int i = 0; i < NELEMENTS; i++) {
		for (int j = 0; j < NELEMENTS; j++) {
			idq = mtqc[j]; idqp1 = mtqc[j+1];
			idc = mtcp[i]; idcp1 = mtcp[i+1];
			idcj = mtcp[j]; idcjp1 = mtcp[j+1];

	        // Compute induced velocities due to vortex AB
			indvel(tem.vecNot(idq,idqp1-1,0),tem.vecNot(idq,idqp1-1,1),
			       tem.vecNot(idq,idqp1-1,2),qcusm.vecNot(idcj,idcjp1),
			       qcm.vecNot(idq,idqp1-1,1),qcm.vecNot(idq,idqp1-1,2),
			       cpm.vecNot(idc,idcp1,0),cpm.vecNot(idc,idcp1,1),
			       cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocities due to vortex BC
	        indvel(qcusm.vecNot(idcj,idcjp1),qcm.vecNot(idq,idqp1-1,1),
	        	   qcm.vecNot(idq,idqp1-1,2),qcusm.vecNot(idcj,idcjp1),
	        	   qcm.vecNot(idq+1,idqp1,1),qcm.vecNot(idq+1,idqp1,2),
	        	   cpm.vecNot(idc,idcp1,0),cpm.vecNot(idc,idcp1,1),
	        	   cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocities due to vortex CD
			indvel(qcusm.vecNot(idcj,idcjp1),qcm.vecNot(idq+1,idqp1,1),
			       qcm.vecNot(idq+1,idqp1,2),tem.vecNot(idq+1,idqp1,0),
			       tem.vecNot(idq+1,idqp1,1),tem.vecNot(idq+1,idqp1,2),
			       cpm.vecNot(idc,idcp1,0),cpm.vecNot(idc,idcp1,1),
			       cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocites from semi-infinite A vortex
	        dx = 1e10;
	        indvel(tem.vecNot(idq,idqp1-1,0)-dx,tem.vecNot(idq,idqp1-1,1),
	        	   tem.vecNot(idq,idqp1-1,2),tem.vecNot(idq,idqp1-1,0),
	        	   tem.vecNot(idq,idqp1-1,1),tem.vecNot(idq,idqp1-1,2),
	        	   cpm.vecNot(idc,idcp1,0),cpm.vecNot(idc,idcp1,1),
	        	   cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocites from semi-infinite D vortex
	        indvel(tem.vecNot(idq+1,idqp1,0),tem.vecNot(idq+1,idqp1,1),
	               tem.vecNot(idq+1,idqp1,2),tem.vecNot(idq+1,idqp1,0)-dx,
	               tem.vecNot(idq+1,idqp1,1),tem.vecNot(idq+1,idqp1,2),
	               cpm.vecNot(idc,idcp1,0),cpm.vecNot(idc,idcp1,1),
	               cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}
		}
	}

	//-----------------------------------------------------------------------
	//     Create AIC matrix. Project downwash along panel normal
	//-----------------------------------------------------------------------

	idc = mtcp[NELEMENTS];
	for (int j = 0; j < idc; j++) {
		for (int i = 0; i < idc; i++) {
			AIC(i, j) = -vdw(i, j) * sin(dihm(i)) - wdw(i, j) * cos(dihm(i));
		}
	}


	//-----------------------------------------------------------------------
	//     Compute velocities at the center of the bound vortex where
	//	   the force applies
	//-----------------------------------------------------------------------
	udw.initConst(0.0);
	vdw.initConst(0.0);
	wdw.initConst(0.0);
	for (int i = 0; i < NELEMENTS; i++) {
		for (int j = 0; j < NELEMENTS; j++) {
			idq = mtqc[j]; idqp1 = mtqc[j+1];
			idc = mtcp[i]; idcp1 = mtcp[i+1];
			idcj = mtcp[j]; idcjp1 = mtcp[j+1];

	        // Compute induced velocities due to vortex AB
			indvel(tem.vecNot(idq,idqp1-1,0),tem.vecNot(idq,idqp1-1,1),
			       tem.vecNot(idq,idqp1-1,2),qcusm.vecNot(idcj,idcjp1),
			       qcm.vecNot(idq,idqp1-1,1),qcm.vecNot(idq,idqp1-1,2),
			       qcusm.vecNot(idc,idcp1),cpm.vecNot(idc,idcp1,1),
			       cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocities due to vortex CD
			indvel(qcusm.vecNot(idcj,idcjp1),qcm.vecNot(idq+1,idqp1,1),
			       qcm.vecNot(idq+1,idqp1,2),tem.vecNot(idq+1,idqp1,0),
			       tem.vecNot(idq+1,idqp1,1),tem.vecNot(idq+1,idqp1,2),
			       qcusm.vecNot(idc,idcp1),cpm.vecNot(idc,idcp1,1),
			       cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocites from semi-infinite A vortex
	        dx = 1e10;
	        indvel(tem.vecNot(idq,idqp1-1,0)-dx,tem.vecNot(idq,idqp1-1,1),
	        	   tem.vecNot(idq,idqp1-1,2),tem.vecNot(idq,idqp1-1,0),
	        	   tem.vecNot(idq,idqp1-1,1),tem.vecNot(idq,idqp1-1,2),
	        	   qcusm.vecNot(idc,idcp1),cpm.vecNot(idc,idcp1,1),
	        	   cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}

	        // Compute induced velocites from semi-infinite D vortex
	        indvel(tem.vecNot(idq+1,idqp1,0),tem.vecNot(idq+1,idqp1,1),
	               tem.vecNot(idq+1,idqp1,2),tem.vecNot(idq+1,idqp1,0)-dx,
	               tem.vecNot(idq+1,idqp1,1),tem.vecNot(idq+1,idqp1,2),
	               qcusm.vecNot(idc,idcp1),cpm.vecNot(idc,idcp1,1),
	               cpm.vecNot(idc,idcp1,2),&du,&dv,&dw,mcp[i],mqc[j]-1);

			for (int k = idc; k < idcp1; k++) {
				for (int l = idcj; l < idcjp1; l++) {
					udw(k,l) += du(k-idc,l-idcj);
					vdw(k,l) += dv(k-idc,l-idcj);
					wdw(k,l) += dw(k-idc,l-idcj);
				}
			}
		}
	}

	// Store results

	// Write to matrix of proper size
	idc = mtcp[NELEMENTS];
	Matrix tmp(idc,idc);
	for (int j = 0; j < idc; j++) {
		for (int i = 0; i < idc; i++) {
			tmp(i,j) = AIC(i,j);
		}
	}
	Matrix AICinv = tmp.inv();
	for (int i = 0; i < idc; i++) {
		for (int j = 0; j < idc; j++) {
			*(ac.AICinv+i+j*idc) = AICinv(i,j);
			*(ac.udw+i+j*idc) = udw(i,j);
			*(ac.vdw+i+j*idc) = vdw(i,j);
			*(ac.wdw+i+j*idc) = wdw(i,j);
		}
	}
}

void indvel(Vector1D xa, Vector1D ya, Vector1D za, Vector1D xb, Vector1D yb,
		Vector1D zb, Vector1D xc, Vector1D yc, Vector1D zc, Matrix* u, Matrix* v,
		Matrix* w, int m, int n) {

	double xca, xcb, xba, yca, ycb, yba, zca, zcb, zba, denom, f1, f2, df;

	for (int i = 0; i < m; i++) {
		for (int j = 0; j < n; j++) {
	          xca = xc(i) - xa(j);
	          xcb = xc(i) - xb(j);
	          xba = xb(j) - xa(j);
	          yca = yc(i) - ya(j);
	          ycb = yc(i) - yb(j);
	          yba = yb(j) - ya(j);
	          zca = zc(i) - za(j);
	          zcb = zc(i) - zb(j);
	          zba = zb(j) - za(j);
	          denom = pow((yca*zcb-ycb*zca),2) + pow((xca*zcb-xcb*zca),2) +
	        		  pow((xca*ycb-xcb*yca),2);
//	          cout << "(i,j) = " << "(" << i << "," << j << ")" << " " << denom << endl;
	          // Check for self inductance
	          if (denom > 1e-7) {
	             f1 = (xba*xca+yba*yca+zba*zca)/sqrt(pow(xca,2)+pow(yca,2)+pow(zca,2));
	             f2 = (xba*xcb+yba*ycb+zba*zcb)/sqrt(pow(xcb,2)+pow(ycb,2)+pow(zcb,2));
	             df = f1 - f2;
	             u->operator ()(i,j) = 1/(4*PI)*(yca*zcb-ycb*zca)/denom*df;
	             v->operator ()(i,j) = 1/(4*PI)*(-xca*zcb+xcb*zca)/denom*df;
	             w->operator ()(i,j) = 1/(4*PI)*(xca*ycb-xcb*yca)/denom*df;
	          } else {
	             u->operator ()(i,j) = 0;
	             v->operator ()(i,j) = 0;
	             w->operator ()(i,j) = 0;
	          }
		}
	}
}
